#include "QMR_precond_CRS.h"

void QMR_precond_CRS(double complex *val, int *col_ind, int *row_ptr, 
		     double complex *b, 
		     double complex *x0, 
		     int n, int maxIter, double tolerance)

{
  int i=0, j=0, k=0;
  int col=0;
  
  double complex lo, xi, gamma, eta, delta, epsilon, beta, lo1, theta, theta1, gamma1; 
 
  //  double tolerance=globalMaxwellTol; 
  int iter=0;
  //  int maxIter = 1000;
  double maxm=1, maxb;
  double complex temp=0, denom=0; 
  double norm_r0;
  double maxm2=0;
  int m=0;
  
  int NoPrecon=1;
  int DILU=0, ILU0=0, SSOR=0, ICUU=0;
  
  double complex *r, *x, *v_star, *y, *w_star, *z, *v, *w;
  double complex *y_star, *z_star, *p, *q, *p_star, *d, *s, *t;
  r=(double complex*)malloc(n*sizeof(double complex));
  x=(double complex*)malloc(n*sizeof(double complex));
  v_star=(double complex*)malloc(n*sizeof(double complex));
  y=(double complex*)malloc(n*sizeof(double complex)); 
  w_star=(double complex*)malloc(n*sizeof(double complex)); 
  z=(double complex*)malloc(n*sizeof(double complex)); 
  v=(double complex*)malloc(n*sizeof(double complex)); 
  w=(double complex*)malloc(n*sizeof(double complex)); 
  y_star=(double complex*)malloc(n*sizeof(double complex));  
  z_star=(double complex*)malloc(n*sizeof(double complex)); 
  p=(double complex*)malloc(n*sizeof(double complex)); 
  q=(double complex*)malloc(n*sizeof(double complex));
  p_star=(double complex*)malloc(n*sizeof(double complex)); 
  d=(double complex*)malloc(n*sizeof(double complex)); 
  s=(double complex*)malloc(n*sizeof(double complex));
  t=(double complex*)malloc(n*sizeof(double complex));

  //ssor
  double para=1.50;
   
  //* D-ILU 
  double complex *Precon=(double complex *)malloc((row_ptr[n])*sizeof(double complex));

  int *diag_ptr=(int *)malloc(n*sizeof(int));

  double complex *pivots=(double complex *)malloc(n*sizeof(double complex));

  double complex *Kcol=(double complex *)malloc(n*sizeof(double complex));

  int found=0;

  double complex element=0; 

  for(i=0;i<n;i++){
     for(j=row_ptr[i];j<row_ptr[i+1];j++){
        if(col_ind[j]==i){
          diag_ptr[i]=j; 
	  pivots[i]=val[j]; 
          break;
        }
     }
     if(cabs(pivots[i])<1.0e-16){ 
       printf("wrong in QMR, zero diagonal\n"); 
       getchar(); 
       return;
     }
  }

  //diagonal scaling
  /*
  for(i=0;i<n;i++){
     for(j=row_ptr[i];j<row_ptr[i+1];j++){ 
         val[j]=val[j]/pivots[i];
     } 
     b[i]=b[i]/pivots[i];
  }
  for(i=0;i<n;i++){
     pivots[i]=1;
  }
  //*/

  for(i=0;i<row_ptr[n];i++){ Precon[i]=val[i];} 

  //diagonal scaling
  /*
  for(i=0;i<n;i++){
     for(j=row_ptr[i];j<row_ptr[i+1];j++){ 
         Precon[j]=Precon[j]/pivots[i];
     }  
  }
  for(i=0;i<n;i++){
     pivots[i]=1;
  }
  //*/

  //D-ILU
  if(DILU==1){
  for(i=0;i<n;i++){
     pivots[i]=1.0/pivots[i];
     for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
        found=0;
        for(k=row_ptr[col_ind[j]];k<diag_ptr[col_ind[j]];k++){
           if(col_ind[k]==i){
             found=1;
             element=Precon[k];
           }
        }
        if(found==1){
          Precon[diag_ptr[col_ind[j]]]=Precon[diag_ptr[col_ind[j]]]-element*pivots[i]*Precon[j];
        }
     }
  }
  }

  //ILU(0)
  if(ILU0==1){  
  }


  //Cholesky 
  if(ICUU==1){  
  for(k=0;k<n;k++){

     for(j=0;j<n;j++){ Kcol[j]=0;}
     for(j=row_ptr[k];j<row_ptr[k+1];j++){
        Kcol[col_ind[j]]=Precon[j];
     }

     for(j=diag_ptr[k]+1;j<row_ptr[k+1];j++){
        element=Precon[j];
        for(i=diag_ptr[col_ind[j]];i<row_ptr[col_ind[j]+1];i++){
           Precon[i]=Precon[i]-Kcol[col_ind[i]]*element/Precon[diag_ptr[k]];
        }
     }

     element=csqrt(Precon[diag_ptr[k]]);
     for(j=diag_ptr[k];j<row_ptr[k+1];j++){
        Precon[j]=Precon[j]/element;
     }
  }  
  }

  if(SSOR==1){
    for(i=0;i<n;i++){
      pivots[i]=1.0/pivots[i];
    }
  }
   
  // x0 initial guess
  for(k=0;k<n;k++){
     x[k]=x0[k]; 
  }

  //1 r0 = b - Ax0
  for(i=0;i<n;i++){
     temp=0;
     for(j=row_ptr[i];j<row_ptr[i+1];j++){
        temp=temp+val[j]*x[col_ind[j]];
     }
     r[i]=b[i]-temp; 
  } 

  //check initial guess
  temp=0; 
  for(i=0;i<n;i++){
     temp=temp+conj(r[i])*r[i];
  }
  temp=csqrt(cabs(temp));
  norm_r0=cabs(temp);
  //printf("norm_r0 = %.10E\n",norm_r0);
  if(cabs(temp)<1e-8){ printf(" Good Initial guess\n"); goto ending;}

  //che vAv
  //denom=0;
  //for(i=0;i<n;i++){
  //   temp=0;
  //   for(j=row_ptr[i];j<row_ptr[i+1];j++){
  //      temp=temp+val[j]*r[col_ind[j]];
  //   }
  //   denom=denom+conj(r[i])*temp; 
  //} 

  //printf("%lf, %lf\n",creal(denom),cimag(denom));

  //2 v*
  for(i=0;i<n;i++){ v_star[i]=r[i];}
  //solve M1 y = v*;
  if(NoPrecon==1){
    for(i=0;i<n;i++){ y[i]=v_star[i];}
  }
  // D-ILU
  if(DILU==1){
    for(i=0;i<n;i++){
       temp=0;
       for(j=row_ptr[i];j<diag_ptr[i];j++){
          temp=temp+Precon[j]*y[col_ind[j]];
       }
       y[i]=(v_star[i]-temp)*pivots[i];
    } 
  }
  // IILU(0)
  if(ILU0==1){
    for(i=0;i<n;i++){
       temp=0;
       for(j=row_ptr[i];j<diag_ptr[i];j++){
          temp=temp+Precon[j]*y[col_ind[j]];
       }
       y[i]=(v_star[i]-temp);
    } 
  } 
  //SSOR
  if(SSOR==1){        
    for(i=0;i<n;i++){
       temp=0;
       for(j=row_ptr[i];j<diag_ptr[i];j++){
          temp=temp+para*Precon[j]*y[col_ind[j]];
       }
       y[i]=(para*(2-para)*v_star[i]-temp)*pivots[i];
    } 
  }
  // IC 
  if(ICUU==1){ 
    for(i=0;i<n;i++){ Kcol[i]=v_star[i];}
    for(i=0;i<n;i++){
       y[i]=Kcol[i]/Precon[diag_ptr[i]]; 
       for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
          Kcol[col_ind[j]]=Kcol[col_ind[j]]-y[i]*Precon[j]; 
       } 
    } 
  }

  temp=0; for(i=0;i<n;i++){ temp=temp+y[i]*conj(y[i]);} 
  lo=csqrt(cabs(temp));

  //3 choose w*
  for(i=0;i<n;i++){
     w_star[i]=r[i];
  }

  
  //4 solve M2^trans z = w* 
  if(NoPrecon==1){
    for(i=0;i<n;i++){ z[i]=w_star[i];}
  }
  // D-ILU
  if(DILU==1){ 
    for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
    for(i=0;i<n;i++){
        z[i]=Kcol[i]; 
        for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
           Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]*pivots[i]); 
        } 
    } 
  }
  // IILU(0)
  if(ILU0==1){
    for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
    for(i=0;i<n;i++){
        z[i]=Kcol[i]/conj(Precon[diag_ptr[i]]); 
        for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
           Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]); 
        } 
    } 
  } 
  //SSOR
  if(SSOR==1){ 
    for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
    for(i=0;i<n;i++){
        z[i]=Kcol[i]; 
        for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
           Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*para*conj(Precon[j]*pivots[i]); 
        } 
    } 
  }
  // IC 
  if(ICUU==1){ 
    for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
    for(i=0;i<n;i++){
        z[i]=Kcol[i]/conj(Precon[diag_ptr[i]]); 
        for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
           Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]); 
        } 
    } 
  } 

  temp=0; for(i=0;i<n;i++){ temp=temp+z[i]*conj(z[i]);} 
  xi=csqrt(cabs(temp));
    
  //5 
  gamma=1; eta=-1;

  //printf("%lf, %lf\n",creal(lo),cimag(lo));
  //printf("%lf, %lf\n",creal(xi),cimag(xi));

  //6 Iterations 
  for(iter=1;iter<=maxIter;iter++){
     //7
     if(cabs(lo)==0 || cabs(xi)==0){ printf("Method faisl at 7 \n"); goto ending;}

     //8
     for(i=0;i<n;i++){ v[i]=v_star[i]/lo; y[i]=y[i]/lo;  }
     //9
     for(i=0;i<n;i++){ w[i]=w_star[i]/xi; z[i]=z[i]/xi;  }
     //10
     temp=0; 
     for(i=0;i<n;i++){temp=temp+conj(z[i])*y[i];} 
     delta=temp;

     //printf("%lf, %lf\n",creal(lo),cimag(lo));
     //printf("%lf, %lf\n",creal(xi),cimag(xi));
     //printf("%lf, %lf\n",creal(delta),cimag(delta));

     if(cabs(delta)==0){ printf("Method faisl at 10 \n"); goto ending;}
     //11 solve M2 y* = y;
     if(NoPrecon==1){
       for(i=0;i<n;i++){ y_star[i]=y[i];}
     }
     // D-ILU
     if(DILU==1){ 
       for(i=n-1;i>=0;i--){
          temp=0;
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             temp=temp+Precon[j]*y_star[col_ind[j]];
          } 
          y_star[i]=y[i]-temp*pivots[i];
       }
     }
     // IILU(0)
     if(ILU0==1){ 
       for(i=n-1;i>=0;i--){
          temp=0;
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             temp=temp+Precon[j]*y_star[col_ind[j]];
          } 
          y_star[i]=(y[i]-temp)/Precon[diag_ptr[i]];
       }
     } 
     //SSOR
     if(SSOR==1){  
       for(i=n-1;i>=0;i--){
          temp=0;
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             temp=temp+para*Precon[j]*y_star[col_ind[j]];
          }  
          y_star[i]=y[i]-temp*pivots[i];
       }
     }
     // IC 
     if(ICUU==1){ 
       for(i=n-1;i>=0;i--){
          temp=0;
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             temp=temp+Precon[j]*y_star[col_ind[j]];
          } 
          y_star[i]=(y[i]-temp)/Precon[diag_ptr[i]];
       }
     } 

     //12 solve M1^transpose z* = z;
     if(NoPrecon==1){
       for(i=0;i<n;i++){ z_star[i]=z[i];}
     }
     // D-ILU
     if(DILU==1){ 
       for(i=0;i<n;i++){ Kcol[i]=z[i];}
       for(i=n-1;i>=0;i--){
          z_star[i]=Kcol[i]*conj(pivots[i]);
          for(j=row_ptr[i];j<diag_ptr[i];j++){
             Kcol[col_ind[j]]=Kcol[col_ind[j]]-conj(Precon[j])*z_star[i]; 
          }  
       }
     }
     // IILU(0)
     if(ILU0==1){   
         for(i=0;i<n;i++){ Kcol[i]=z[i];}
         for(i=n-1;i>=0;i--){
            z_star[i]=Kcol[i];
            for(j=row_ptr[i];j<diag_ptr[i];j++){
               Kcol[col_ind[j]]=Kcol[col_ind[j]]-conj(Precon[j])*z_star[i]; 
            }  
         }
     } 
     //SSOR
     if(SSOR==1){   
         for(i=0;i<n;i++){ Kcol[i]=para*(2-para)*z[i];}
         for(i=n-1;i>=0;i--){
            z_star[i]=Kcol[i]*conj(pivots[i]);
            for(j=row_ptr[i];j<diag_ptr[i];j++){
               Kcol[col_ind[j]]=Kcol[col_ind[j]]-para*conj(Precon[j])*z_star[i]; 
            }  
         }  
     }
     // IC 
     if(ICUU==1){ 
       for(i=n-1;i>=0;i--){
          temp=0;
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             temp=temp+conj(Precon[j])*z_star[col_ind[j]];
          } 
          z_star[i]=(z[i]-temp)/conj(Precon[diag_ptr[i]]);
       }
     }
 

     //13
     if(iter==1){
       for(i=0;i<n;i++){
          p[i]=y_star[i]; q[i]=z_star[i];
       }      
     }
     else{
       temp=xi*delta/epsilon;
       for(i=0;i<n;i++){
          p[i]=y_star[i]-temp*p[i];
       }
       temp=lo*conj(delta/epsilon);
       for(i=0;i<n;i++){
          q[i]=z_star[i]-temp*q[i];
       }
     }
     //19 p_star = A p
     for(i=0;i<n;i++){
        temp=0;
        for(j=row_ptr[i];j<row_ptr[i+1];j++){
           temp=temp+val[j]*p[col_ind[j]];
        }
        p_star[i]=temp; 
     } 

     //20
     temp=0; 
     for(i=0;i<n;i++){ temp=temp+conj(q[i])*p_star[i];} 
     epsilon=temp;
     if(cabs(epsilon)==0){ printf("Method faisl at 20 \n"); goto ending;}
     //21
     beta=epsilon/delta; 
     if(cabs(beta)==0){ printf("Method faisl at 21 \n"); goto ending;}
     //22
     for(i=0;i<n;i++){ v_star[i]=p_star[i]-beta*v[i];}
     
     //23 solve M1 y = v*
     if(NoPrecon==1){
       for(i=0;i<n;i++){ y[i]=v_star[i];}
     }
     // D-ILU
     if(DILU==1){
       for(i=0;i<n;i++){
          temp=0;
          for(j=row_ptr[i];j<diag_ptr[i];j++){
             temp=temp+Precon[j]*y[col_ind[j]];
          }
          y[i]=(v_star[i]-temp)*pivots[i];
        } 
     }
     // IILU(0)
     if(ILU0==1){
       for(i=0;i<n;i++){
          temp=0;
          for(j=row_ptr[i];j<diag_ptr[i];j++){
             temp=temp+Precon[j]*y[col_ind[j]];
          }
          y[i]=(v_star[i]-temp);
       } 
     } 
     //SSOR
     if(SSOR==1){        
       for(i=0;i<n;i++){
          temp=0;
          for(j=row_ptr[i];j<diag_ptr[i];j++){
             temp=temp+para*Precon[j]*y[col_ind[j]];
          }
          y[i]=(para*(2-para)*v_star[i]-temp)*pivots[i];
       } 
     }
     // IC 
     if(ICUU==1){ 
       for(i=0;i<n;i++){ Kcol[i]=v_star[i];}
       for(i=0;i<n;i++){
          y[i]=Kcol[i]/Precon[diag_ptr[i]]; 
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             Kcol[col_ind[j]]=Kcol[col_ind[j]]-y[i]*Precon[j]; 
          } 
       } 
     }

   
     //24
     temp=0; for(i=0;i<n;i++){ temp=temp+conj(y[i])*y[i];}
     lo1=csqrt(cabs(temp));
     //25  get t = A^transpose q
     for(i=0;i<n;i++){
        t[i]=0;
     }
     for(j=0;j<n;j++){
        for(i=row_ptr[j];i<row_ptr[j+1];i++){
           t[col_ind[i]]=t[col_ind[i]]+conj(val[i])*q[j];
        }
     }
     //25
     for(i=0;i<n;i++){ w_star[i]=t[i]-conj(beta)*w[i];}

     //26 solve M2^transpose z = w*
     if(NoPrecon==1){
       for(i=0;i<n;i++){ z[i]=w_star[i];}
     }
     // D-ILU
     if(DILU==1){ 
       for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
       for(i=0;i<n;i++){
          z[i]=Kcol[i]; 
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]*pivots[i]); 
          } 
       } 
     }
     // IILU(0)
     if(ILU0==1){
       for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
       for(i=0;i<n;i++){
           z[i]=Kcol[i]/conj(Precon[diag_ptr[i]]); 
           for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
              Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]); 
           } 
       } 
     } 
     //SSOR
     if(SSOR==1){ 
       for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
       for(i=0;i<n;i++){
          z[i]=Kcol[i]; 
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*para*conj(Precon[j]*pivots[i]); 
          } 
       } 
     }
     // IC 
     if(ICUU==1){ 
       for(i=0;i<n;i++){ Kcol[i]=w_star[i];}  
       for(i=0;i<n;i++){
          z[i]=Kcol[i]/conj(Precon[diag_ptr[i]]); 
          for(j=diag_ptr[i]+1;j<row_ptr[i+1];j++){
             Kcol[col_ind[j]]=Kcol[col_ind[j]]-z[i]*conj(Precon[j]); 
          } 
       } 
     }  

     //27
     temp=0; for(i=0;i<n;i++){ temp=temp+conj(z[i])*z[i];}
     xi=csqrt(cabs(temp));

     //28
     theta1=lo1/(gamma*cabs(beta));
     gamma1=1.0/csqrt(1.0+theta1*theta1); 
     if(cabs(gamma1)==0){ printf("Method faisl at 28 \n"); goto ending;}
     
     //29
     eta=-eta*lo*gamma1*gamma1/(beta*gamma*gamma);

     //30
     if(iter==1){
       for(i=0;i<n;i++){
          d[i]=eta*p[i]; s[i]=eta*p_star[i];
       }
     }
     else{
       temp=theta*gamma1*theta*gamma1; 
       for(i=0;i<n;i++){
          d[i]=eta*p[i]+temp*d[i];
          s[i]=eta*p_star[i]+temp*s[i]; 
       }
     } 
     //36
     for(i=0;i<n;i++){
        x[i]=x[i]+d[i];
        r[i]=r[i]-s[i];
     }

     //37 check convergence
     maxm=0;
     for(i=0;i<n;i++){
       maxm = cabs(d[i]) > maxm ? cabs(d[i]) : maxm ;
     }
     //maxm2=0;
     //for(i=0;i<n;i++){
     //   maxm2=max2(maxm2,cabs(r[i]));
     //}
     temp=0;
     for(i=0;i<n;i++){
        temp=temp+conj(r[i])*r[i];
     }
     maxm2=sqrt(cabs(temp));
 
     if(0){
     denom=0;
     for(i=0;i<n;i++){
        temp=0;
        for(j=row_ptr[i];j<row_ptr[i+1];j++){
           temp=temp+val[j]*x[col_ind[j]];
        }
        denom=denom+(b[i]-temp)*conj(b[i]-temp);  
     } 
     maxm2=sqrt(cabs(denom));
     }

     if(iter % 20 == 0)
       printf("iter = %4d;  x_tol = %.10E;  r_tol = %.10E\n", iter, maxm, maxm2);
     //if(maxm<tolerance*1.0e-10 || maxm2/norm_r0<tolerance){ goto ending;}
     if(maxm<1.0e-15 || maxm2/norm_r0<tolerance){ goto ending;}
     //if(maxm2/norm_r0<tolerance){ goto ending;}

     //36 restore lo, gamma, theta
     lo=lo1; gamma=gamma1; theta=theta1; 
  }
  
ending:

  for(k=0;k<n;k++){
     x0[k]=x[k];
  }  

  free(r); free(x); free(v_star); free(y); 
  free(w_star); free(z); free(v); free(w); 
  free(y_star); free(z_star);  free(p); free(q); 
  free(p_star); free(d); free(s); free(t);

  free(Precon); free(diag_ptr); free(pivots);  free(Kcol); 

  if(iter>=maxIter-1){ printf("Maximum iterations reached in QMR \n"); }

  return 0;
} 
       
